#include "../inc/filter_hough_circle.h"
#include "../inc/Tools.h"
#include "../../filter_PCA/filter_PCA.h"
#include <math.h>
#include <vcg/complex/trimesh/append.h>
#include <vcg/complex/trimesh/allocate.h>
#include <vcg/complex/trimesh/create/platonic.h>
//#include <wrap/gl/addons.h>

#define twoPi  (2 * M_PI)
#define sections  40	 /* Nombre de sections utilises pour dessiner le cercle */

Filter_Hough_Circle::Filter_Hough_Circle()
{
	typeList << FP_HOUGH_CIRCLE;
	
	foreach(FilterIDType tt , types())
		actionList << new QAction(filterName(tt), this);
	
	ptsMin.clear();
	mesh = NULL;
}

QString Filter_Hough_Circle::filterName(FilterIDType filterId) const
{
  switch(filterId) {
		case FP_HOUGH_CIRCLE :  return QString("Hough Circles"); 
		default : assert(0); 
	}
  return QString();
}

QString Filter_Hough_Circle::filterInfo(FilterIDType filterId) const
{
  switch(filterId) {
		case FP_HOUGH_CIRCLE :  return QString("PENSER A PASSER EN VISUALISATION DE POINTS POUR VOIR LE RESULTAT"); 
		default : assert(0); 
	}
	return QString("Unknown Filter");
}

Filter_Hough_Circle::FilterClass Filter_Hough_Circle::getClass(QAction *a)
{
  switch(ID(a))
	{
	case FP_HOUGH_CIRCLE :  return MeshFilterInterface::PointSet; 
		default : assert(0); 
	}
	return MeshFilterInterface::Generic;
}

void Filter_Hough_Circle::initParameterSet(QAction *action,MeshModel &m, RichParameterSet & parlst) 
{
	 switch(ID(action))	 {
		case FP_HOUGH_CIRCLE :  
			parlst.addParam(new RichInt ("resX",
										20,
										"Resolution de la grille en x",
										"Nombre de cases de la grille selon l'axe Ox"));
			
			parlst.addParam(new RichInt ("resY",
										20,
										"Resolution de la grille en Y",
										"Nombre de cases de la grille selon l'axe Oy"));
			
			parlst.addParam(new RichFloat ("lowThresh",
										1.3,
										"Seuil bas de la coupe",
										"Altitude minimum des points a garder dans la coupe"));
			
			parlst.addParam(new RichFloat ("highTresh",
										1.6,
										"Seuil haut de la coupe",
										"Altitude maximum des points a garder dans la coupe"));

			parlst.addParam(new RichInt ("nbPtsMax",
										20,
										"Nombre de points max par coupe dans une cellule",
										"Nombre de points max par coupe dans une cellule"));

			
			parlst.addParam(new RichFloat ("rMax",
										0.3,
										"Rayon max des cercles à chercher",
										"Les cercles trouvés de rayons supérieur à celui-ci seront ignorés"));
			
			parlst.addParam(new RichInt ("resHoughA",
										50,
										"Resolution des espaces de Hough selon A",
										"Nombre de cases de l'espace de Hough de chaque cellule de la grille pour l'axe A"));
			
			parlst.addParam(new RichInt ("resHoughB",
										50,
										"Resolution des espaces de Hough selon B",
										"Nombre de cases de l'espace de Hough de chaque cellule de la grille pour l'axe B"));
			
			parlst.addParam(new RichInt ("resHoughR",
										50,
										"Resolution des espaces de Hough selon R",
										"Nombre de cases de l'espace de Hough de chaque cellule de la grille pour l'axe R"));
			
			parlst.addParam(new RichInt ("houghSeuil",
										5,
										"Seuil de l'espace de Hough",
										"Seuil au dessus duquel les cases de l'espace de Hough sont considerees comme pertinentes"));
			
			parlst.addParam(new RichString ("file",
										"cercles.txt",
										"Fichier dans lequel enregistrer les cercles",
										"Les cercles seront enregistrés dans ce fichier pour être ensuite affichés"));
			
										break;
											
		default : assert(0); 
	}
}

QString Filter_Hough_Circle::filterScriptFunctionName( FilterIDType filterID )
{
	switch(filterID) {
		case FP_HOUGH_CIRCLE :  return QString("filter_hough_circle");
		default : assert(0); 
	}
	return QString();
}

bool Filter_Hough_Circle::applyFilter(QAction */*filter*/, MeshDocument &md, RichParameterSet & par, vcg::CallBackPos *cb)
{
	vector<Circle> rslt;
	
	/* On initialise ce dont on a besoin pour le filtre */
	init_filter( md, par );
	
	/* On fait la coupe du nuage */
	if ( !sliceMesh( par.getFloat("lowThresh"), par.getFloat("highTresh"), cb, par.getInt("nbPtsMax") ) )
		return false;
	
	/* On enregistre les cercles résultats dans un fichier texte */
	rslt = hsCloud(par, cb);
	HoughSpace::saveCircles( rslt, par.getString("file") );
	
	/* On crée un nouvea layer qui contient les cercles trouvés par Hough */
	MeshModel* secondMesh = createCirclesLayer( md, rslt );
	
	/* On libère l'espace utilisé par la copie locale du maillage */
	mesh->cm.Clear();
	
	return true;
}

void Filter_Hough_Circle::init_meshModel(MeshDocument &md)
{
	/* On alloue l'espace pour la copie en local du nuage de point */
	mesh = new MeshModel (&md, QString("fileName"), QString ("label_cloudCopy") );
	
	/* On fait une copie "en local" du nuage de point d'entrée du plugin */
	vcg::tri::Append<CMeshO,CMeshO>::Mesh( mesh->cm, md.mm()->cm );
	
	/* On met à jour sa bounding box */
	vcg::tri::UpdateBounding<CMeshO>::Box(mesh->cm);
}

void Filter_Hough_Circle::init_grid(MeshModel *_mesh, RichParameterSet &par, int _resZ)
{
	/* On plonge le nuage dans la grille */
	grid.Set( _mesh->cm.vert.begin(), _mesh->cm.vert.end(), _mesh->cm.bbox, vcg::Point3i( par.getInt("resX"), par.getInt("resY"), _resZ ) );
}

void Filter_Hough_Circle::init_grid2(MeshModel *_mesh, RichParameterSet &par, int _resZ)
{
	/* Calcul de la taille des cellules de la deuxième grille */
	float cellSizeX = (_mesh->cm.bbox.max[0] - _mesh->cm.bbox.min[0]) / (float)par.getInt("resX");
	float cellSizeY = (_mesh->cm.bbox.max[1] - _mesh->cm.bbox.min[1]) / (float)par.getInt("resY");
	
	/* On prends une bounding box plus large de une largeur de case dans selon axe pour notre deuxième grille */
	vcg::GridStaticPtr<CVertexO>::Box3x newBbox = _mesh->cm.bbox;
	newBbox.Offset( vcg::Point3f( cellSizeX, cellSizeY, 0 ) );
	
	/* On plonge le nuage dans la deuxième grille */
	grid2.Set( _mesh->cm.vert.begin(), _mesh->cm.vert.end(), newBbox, vcg::Point3i( par.getInt("resX")+1, par.getInt("resY")+1, _resZ ) );
}

void Filter_Hough_Circle::init_ptsMin( const vcg::GridStaticPtr<CVertexO, float>& _grid )
{
	/* Pour chaque case de la grille */
	for ( int i = 0 ; i < (int)_grid.grid.size()-1 ; i++ )
	{
		/* On crée un vertex qu'on va placer dans le tableau ptsMin */
		CVertexO vMin;
		/* On initialise sa hauteur à max pour rechercher ensuite la hauteur minimum */
		vMin.P() = vcg::Point3f(0,0,LONG_MAX);
		
		/* On recupere un pointeur vers le premier link de la cellule et vers le dernier */
		vcg::GridStaticPtr<CVertexO,float>::Cell start, end;
		grid.Grid( &(grid.grid[i]), start, end);
		
		/* On parcours les vertex qui la composent */
		for ( vcg::GridStaticPtr<CVertexO,float>::Link* linkIt = start ; linkIt != end ; linkIt++ )
		{
			/* Si la hauteur du point courant est plus petite que la hauteur du point minimum */
			if ( linkIt->Elem()->P()[2] < vMin.P()[2] )
			{
				/* Le point minimum est affecté au point courant */
				vMin.P() = linkIt->Elem()->P();
			}
		}
		
		/* On a trouvé le point d'altitude minimum, il ne reste qu'à l'ajouter au tableau des ptsMin */
		ptsMin.push_back( vMin );
	}
}

void Filter_Hough_Circle::init_filter(MeshDocument &md, RichParameterSet &par )
{
	/* Initialisation du modèle */
	init_meshModel( md );
	
	/* Initialisation de la grille1 en vue de calculer les points minimum de chaque cellule */
	init_grid( this->mesh, par );
	
	//	Utile seulement si on passe par la vérification du flag IsD() : si on a pas réellement supprimé les sommets lors de la coupe
	init_grid2( this->mesh, par );
	
	/* Calcul des points de hauteur minimum dans chaque cellule */
	init_ptsMin( this->grid );
}

bool Filter_Hough_Circle::sliceMesh( float _lowThresh, float _highTresh, vcg::CallBackPos *cb, int _nbPtsInSlice )
{
	/* On déclare un tableau qui contiendra, à chaque cellule, les sommets qui appartiennent à la coupe */
	std::vector<CVertexO*> tabVert;

	/* test des parametres */
	if ( _lowThresh > _highTresh )
		return false;
	
	/* Pour chaque cellule de la grille */
	for ( int i = 0 ; i < (int)grid.grid.size()-1 ; i++ )
	{
		/* On vide le tableau qui contient les sommets de la coupe de la cellule précédente */
		tabVert.clear();
		
		/* MAJ de la barre de progression */
		cb((100*i)/(grid.grid.size()-1),"Slicing the point cloud");
		
		/* On recupere un pointeur vers le premier link de la cellule et vers le dernier */
		vcg::GridStaticPtr<CVertexO,float>::Cell start, end;
		grid.Grid( &(grid.grid[i]), start, end);

		/* On parcours les vertex qui la composent */
		for ( vcg::GridStaticPtr<CVertexO,float>::Link* linkIt = start ; linkIt != end ; linkIt++ )
		{
			/* Si la hauteur du vertex courant dépasse la somme de l'altitude du plus bas point et des seuils donnés */
			if ( linkIt->Elem()->P()[2] < ( _lowThresh + ptsMin[i].P()[2] ) || linkIt->Elem()->P()[2] > ( _highTresh + ptsMin[i].P()[2] ) )
			{
				/* On supprime le sommet du maillage */
				vcg::tri::Allocator<CMeshO>::DeleteVertex( this->mesh->cm, *(linkIt->Elem()) );

			}
			
			/* Sinon, le sommet fait partie de la coupe */
			else
			{
				/* On l'ajoute au tableau qui contient les sommets appartenants à la coupe */
			    tabVert.push_back(linkIt->Elem());
			}
		}
		
		/* Pour ne garder qu'un nombre maximum de sommets, on les trie selon leur altitude pour ensuite ne garder que les X sommets de plus basse altitude */
		sort(tabVert.begin(), tabVert.end(), compareVert);

		/* Si le nb de sommets max est inférieur au nb de sommets de la coupe */
		if( _nbPtsInSlice < (int)tabVert.size() )
		{
		    /*On ne conserve que les _nbPtsInSlice premiers sommets*/
		    for( int j = _nbPtsInSlice ; j < (int)tabVert.size() ; j++ )
		    {
				/* CAD qu'on supprime du maillage les sommets à partir du _nbPtsInSlice ème */
				vcg::tri::Allocator<CMeshO>::DeleteVertex( this->mesh->cm, *(tabVert[j]) );
		    }
		}
	}

	/* MAJ barre de progression */
	cb(100,"Slice done");
	
	return true;
}

vector<Circle> Filter_Hough_Circle::hsCell(vcg::GridStaticPtr<CVertexO,float> _grid, vcg::GridStaticPtr<CVertexO,float>::Cell* _cell, RichParameterSet &par )
{
	/* On déclare un cercle qui sera rempli pour chaque triplet de point de la case */
	Circle c;
	
	/* Si le premier point de la cellule est inexistant */
	if ( (*_cell)->Elem() == NULL )
	{
		/* On renvoie un vecteur de cercle vide */
		vector<Circle> empty;
		return empty;
	}
	
	/* On récupère les coordonnées en 3D de la cellule que l'on obtient grâce au premier point de la cellule (c'est le bordel) */
	vcg::Point3i cellCoord;
	_grid.PToIP( (*_cell)->Elem()->P(), cellCoord );
	
	/* On recupere la bbox de la cellule dans cellBbox */
	vcg::GridStaticPtr<CVertexO,float>::Box3x cellBbox;
	_grid.IPiToBox( cellCoord, cellBbox );
	
	/* On crée un espace de Hough de dimensions :
	   - a compris entre le x minimum de la case et son x maximum
	   - b compris entre le y minimum de la case et son y maximum
	   - r compris entre 0 et le rayon maximum fixé par l'utilisateur.
	   Le nombre de case de l'espace selon ces trois axes est défini par l'utilisateur.
	*/
	HoughSpace hs ( RESOLUTION, Point (cellBbox.min[0], cellBbox.min[1], 0.0), Point ( cellBbox.max[0], cellBbox.max[1], par.getFloat("rMax") ), par.getInt("resHoughA"), par.getInt("resHoughB"), par.getInt("resHoughR"));
	
	/* On recupere un pointeur vers le premier link de la cellule et vers le dernier */
	vcg::GridStaticPtr<CVertexO,float>::Cell start, end;
	_grid.Grid( _cell, start, end);
		
	/* Si on a au moins trois points dans la cellule */
	if ( (start+2) != end && (start+1) != end && start != end )
	{
		/* On parcours tous les triplets de points grâce aux trois boucles for suivantes */
		for ( vcg::GridStaticPtr<CVertexO,float>::Link* linkIt = start ; linkIt != (end-2) ; linkIt++ )
		{
			//	Utile seulement si on passe par la vérification du flag IsD() : si on a pas réellement supprimé les sommets lors de la coupe
			/* Si le sommet n'avait pas été marqué comme à supprimer pour la coupe */
			if ( !linkIt->Elem()->IsD() )
			{
				for ( vcg::GridStaticPtr<CVertexO,float>::Link* linkIt2 = linkIt+1 ; linkIt2 != (end-1) ; linkIt2++ )
				{
					//	Utile seulement si on passe par la vérification du flag IsD() : si on a pas réellement supprimé les sommets lors de la coupe
					/* Si le sommet n'avait pas été marqué comme à supprimer pour la coupe */
					if ( !linkIt2->Elem()->IsD() )
					{
						for ( vcg::GridStaticPtr<CVertexO,float>::Link* linkIt3 = linkIt2+1 ; linkIt3 != end ; linkIt3++ )
						{
							//	Utile seulement si on passe par la vérification du flag IsD() : si on a pas réellement supprimé les sommets lors de la coupe
							/* Si le sommet n'avait pas été marqué comme à supprimer pour la coupe */
							if ( !linkIt3->Elem()->IsD() )
							{
								/* On obtient le cercle formé par le triplet de points */
								c = Circle( (*linkIt->Elem()), (*linkIt2->Elem()), (*linkIt3->Elem()) );
								
								/* Si le rayon du cercle obtenu est valide */
								if ( c.getR() < par.getFloat("rMax") )
								{
									/* On ajoute le cercle dans l'espace de Hough */
									hs.addCircle( c );
								}
							}
						}
					}
				}
			}
		}
	}
	
	/* Pour ne garder que les bons cercles, on seuille l'espace de Hough. On récupere ainsi un tableau de cercles */
	return hs.threshold( par.getInt("houghSeuil") );
}

vector<Circle> Filter_Hough_Circle::hsGrid( vcg::GridStaticPtr<CVertexO,float> _grid, RichParameterSet &par, vcg::CallBackPos *cb )
{
	/* On crée un tableau qui contiendra les cercles en sortie */
	vector<Circle> rslt;
	
	/* Pour chaque cellule de la grille */
	for ( int i = 0 ; i < (int)_grid.grid.size()-1 ; i++ )
	{
		/* Mettre à jour la barre de progression */
		cb((i*100)/_grid.grid.size(),"Calcul des espaces de Hough locaux");
		
		/* On récupere un tableau contenant les cercles pertinent de la grille */
		vector<Circle> circles = hsCell(_grid, &(_grid.grid[(int)i]), par );
		
		/* Que l'on rajoute au tableau résultat */
		copy (circles.begin(),circles.end(),back_inserter(rslt));
	}

	return rslt;
}

vector<Circle> Filter_Hough_Circle::hsCloud(RichParameterSet &par, vcg::CallBackPos *cb)
{
	/* Deux tableaude cercles pour recuperer les cercles des espaces de hough de chaque grille */
	vector<Circle> rsltGrid1;
	vector<Circle> rsltGrid2;
	
	/* On recupere les deux tableaux de cercles */
	rsltGrid1 = hsGrid( grid, par, cb );
	rsltGrid2 = hsGrid( grid2, par, cb );
	
	/* On ajoute le tableau 2 au tableau 1 */
	copy (rsltGrid2.begin(),rsltGrid2.end(),back_inserter(rsltGrid1));
	
	/* On renvoie le resultat */
	return rsltGrid1;
}

MeshModel* Filter_Hough_Circle::createCirclesLayer ( MeshDocument& _parent, vector<Circle> _circles )
{
	//	POUR CREER UN NOUVEAU LAYER IL FAUT CREER UN NOUVEAU MESHMODEL
	/* Que l'on ajoute au meshDocument */
	MeshModel* secondMesh = _parent.addNewMesh("", QString ("circles") );

	
	/* On ajoute les cercles de Hough au layer nouvllement cree */
	for ( int i = 0 ; i < (int)_circles.size() ; i++ )
	{
		Filter_Hough_Circle::addCircleToMesh(secondMesh->cm, _circles[i], 0 );
	}
	
	/* On resiegne sa bounding box */
	vcg::tri::UpdateBounding<CMeshO>::Box(secondMesh->cm);
	
	/* Et on lui donne la même matrice de transfo que l'original (pour la trackball) */
	secondMesh->cm.Tr = _parent.mm()->cm.Tr;
	
	return secondMesh;
}

void Filter_Hough_Circle::addCircleToMesh(CMeshO &_mesh, Circle _c, float _height )
{
	int oldNVert = _mesh.vn;
	int oldnFace = _mesh.fn;
	
	/* On ajoute au mesh le nombre de sommets necessaire pour faire un cercle */
	vcg::tri::Allocator<CMeshO>::AddVertices( _mesh, sections );
	
	/* On renseigne les sommets à ajouter */
	for(int i = 0; i < sections ; i++) 
	{
		_mesh.vert[ oldNVert + i ].P()[0] = _c.getR() * cos(i * twoPi / sections ) + _c.getA();
		_mesh.vert[ oldNVert + i ].P()[1] = _c.getR() * sin(i * twoPi / sections ) + _c.getB();
		_mesh.vert[ oldNVert + i ].P()[2] = _height;
		_mesh.vert[ oldNVert + i ].C() =  vcg::Color4b::Red;
	}
	
	/* On ajoute le centre du cercle */
	vcg::tri::Allocator<CMeshO>::AddVertices( _mesh, 1 );
	_mesh.vert[ _mesh.vn - 1 ].P()[0] = _c.getA();
	_mesh.vert[ _mesh.vn - 1 ].P()[1] = _c.getB();
	_mesh.vert[ _mesh.vn - 1 ].P()[2] = _height;
	_mesh.vert[ _mesh.vn - 1 ].C() =  vcg::Color4b::Red;
	
	
	/* On ajoute les faces */
	vcg::tri::Allocator<CMeshO>::AddFaces( _mesh, sections );
	
	for ( int i = 0 ; i < sections ; i++ )
	{
		_mesh.face[ oldnFace + i ].V(0) = &(_mesh.vert[ oldNVert + i ]);
		_mesh.face[ oldnFace + i ].V(1) = &(_mesh.vert[ oldNVert + ((i+1)%sections) ]);
		_mesh.face[ oldnFace + i ].V(2) = &(_mesh.vert[ _mesh.vn -1 ]);
		_mesh.vert[ oldnFace + i ].C() =  vcg::Color4b::Red;
	}
}

Q_EXPORT_PLUGIN(Filter_Hough_Circle)
